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ABSTRACT 

This  document  describes  several  versions  of  software  that  have  been  developed  to 
implement  an  Inertial  Navigation  System  algorithm.  The  form  of  the  algorithm  is 
intended  to  faciUtate  the  combining  of  Global  Positioning  System  data  into  the  INS 
solution,  and  a  simple  example  (position  and  velocity  reset)  is  included  in  the 
software. 
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Low  Cost  INS-GPS  Integration  for  Navigation 
(INS  Software  Implementation) 


Executive  Summary 

Inertial  Navigation  Systems  (INS)  and  Global  Positioning  Systems  (GPS)  both  can  be 
used  for  a  wide  range  of  navigation  functions.  Each  has  its  strengths  and  weaknesses 
in  this  respect. 

The  major  strengths  of  GPS  are  that  it  can  provide  regular  estimates  of  position  that 
will  remain  within  certain  error  boimds  and  that  its  hardware  costs  are  relatively 
cheap.  Its  weaknesses  are;  the  size  of  the  errors  from  GPS  receivers  (not  requiring 
approval  from  the  US  Department  of  Defense)  are  relatively  large  (aroimd  100  m);  the 
update  rate  for  navigation  solutions  are  relatively  slow  (1  to  10  Hz);  and  the  receiver 
output  is  unreliable  due  to  received  signal  dropping  out  or  being  jammed. 

For  INS  its  strengths  are:  a  high  navigation  solution  rate;  low  errors  over  a  short 
period  of  time;  and  reliability  as  it  is  self  contained.  The  major  weakness  is  that  errors 
grow  with  time  and  the  rate  of  growth  is  inversely  related  to  the  cost  of  the  INS. 

Combined  INS-GPS  systems  are  capable  of  making  up  for  the  weaknesses  inherent  in 
each.  For  example  self  guided  misshes  need  a  high  degree  of  accuracy  in  the  end  game 
scenario  which  may  achieved  with  high  cost  INS  guidance  alone  but  with  the 
incorporation  of  GPS  much  cheaper  INS  units  could  be  used  for  the  same  result. 

This  document  describes  a  software  implementation  of  an  INS  algorithm  that  will 
allow  easy  integration  of  GPS  data  into  the  navigation  solution  taking  into  accoimt 
issues  such  as  different  data  rates  from  the  inertial  sensors  and  GPS  receivers.  This 
software  will  be  useful  for  research  into  optimisation  algorithms  for  INS-GPS 
integration. 
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1.  Introduction 


Guidance  and  Control  Group  of  Weapons  Systems  Division  has  a  task  GPS-Guided 
Weapons  Applications  (ADA93/176)  in  which  research  is  being  undertaken  to  develop 
navigation  techniques  for  self  guided  unmanned  vehicles  based  on  low-cost  Inerbal 
Navigation  System  (INS)  and  Global  Positioning  System  (GPS)  technologies. 


2.  Background 

Strapdown  INS  technologies  are  based  on  the  principle  of  inte^ating  specific  forces 
and  rates  measured  by  accelerometers  and  rate  gyros  of  an  hiertial  Measurement  Umt 
(IMU)  fixed  to  the  navigating  body.  Given  the  initial  conditions  of  position,  velocity 
and  attitude,  accurate  real  time  integration  of  IMU  output  will  produce  position  ^d 
attitude  information  in  some  given  navigation  coordinate  system.  GK  on  the  other 
hand  reUes  on  the  technique  of  comparing  signals  from  orbiting  satelHtes  to  calcuMe 
position  (and  possibly  attitude)  at  regular  time  intervals,  but  being  dependent  on  &e 
sateUites  signals  makes  GPS  less  reliable  than  the  self  contamed  INS  due  to  the 
possibility  of  drop-outs  or  jamming. 

The  errors  inherent  with  INS,  ie  tum-on  bias,  dynamic  biases  and  integration,  are 
relatively  small  over  update  periods,  however  they  accumulate  with  time.  Errors  with 
GPS  are  bounded  with  time,  however  they  are  large  relative  to  those  of  INS  over  an 

update  period. 

The  two  technologies  are  complimentary  so  that  combining  them  provides  an 
opportunity  to  overcome  their  respective  weaknesses.  Combined  INS-GPS  systems 
usually  rely  on  high  accuracy  IMU's  (hence  expensive)  while  using  less  than  optimal 
algorithms  to  combine  the  data.  By  using  improved  algorithms  it  is  believed  that  less 
accurate,  but  relatively  inexpensive  IMU's  could  be  used  in  combined  INS-GPS 
systems. 


3.  Aims 

In  support  of  the  development  of  algorithms  to  combine  INS  and  GPS  data  this 
document  describes  the  software  implementation  of  an  INS  algorithm  the  structure  of 
which  will  allow  the  incorporation  of  GPS  data,  and  that  of  other  sources,  such  as 
magnetometers,  altimeters  etc. 
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4.  Considerations  for  Combining  INS-GPS  Data 

The  following  discusses  some  of  the  specific  issues  peculiar  to  INS  and  GPS  systems 
that  will  have  a  bearing  on  how  a  combined  INS-GPS  system  may  be  implemented. 


4.1  INS 

In  integrating  IMU  data  to  obtain  a  navigation  solution,  the  sampling  period  in  an 
integration  step  is  important  to  consider.  For  a  strapdown  IMU,  body  specific  forces 
and  rates  are  likely  to  have  some  relatively  high  frequency  components  hence  a  high 
sampling  and  integration  rate  is  needed  to  deal  with  these.  On  the  other  hand 
accotmting  for  the  motion  of  the  navigation  reference  frame  (usually  Earth  based 
reference)  with  respect  to  inertial  space  (in  which  an  IMU  operates),  need  only  be 
performed  at  a  much  slower  frequency  since  the  motion  of  the  Earth's  surface  in  space 
is  both  smooth  and  slow  (in  rotation).  Other  considerations  for  IMU  integration  such 
as  the  value  for  gravity  as  a  function  of  altitude  and  latitude  need  only  be  sampled  at 
slower  frequencies. 

Computational  time  becomes  an  issue  given  that  it  is  desirable  to  perform  at  least  some 
of  the  integration  process  at  a  high  rate,  hence  there  is  a  need  to  minimise  the 
computational  over-head.  An  approximate  solution  to  the  equations  of  motion  that 
cuts  computational  time  is  a  compromise  between  these  conflicting  requirements. 

4.2  GPS 

GPS  receivers  can  operate  on  at  least  one  of  two  coded  messages  transmitted  by  the 
sateUites.  One  of  the  messages  is  encrypted  and  is  only  accessible  with  the  approval  of 
the  US  Dept,  of  Defense.  The  other  is  open  to  public  use  however  the  information  it 
contains  has  been  deliberately  corrupted  so  that  the  ultimate  accuracy  of  receivers 
using  it  alone  are  an  order  of  magrutude  worse  than  those  using  the  encrypted 
message,  the  latter  being  capable  of  less  than  10  meters  accuracy. 

It  is  possible  to  counter  the  intentionally  added  errors  using  differential  techniques 
where  a  second  receiver  at  a  known  location  transmits  error  information  to  the  first 
that  is  common  to  both  receivers.  This  is  not  practical  for  the  intended  purposes  of  the 
navigation  system  being  studied  so  need  not  be  considered  further  here. 

Most  GPS  receivers  will  generate  navigation  solutions  at  a  rate  of  1  Hz,  however  some 
receivers  wiU  produce  at  least  a  partial  solution  at  rates  up  to  10  Hz. 

4.3  Combining  INS  and  GPS 

Whatever  technique  is  chosen  to  fuse  the  data  from  the  IMU  and  GPS  units  the  major 
consideration  arising  from  the  previous  sections  is  that  of  the  rates  at  which  data 
becomes  available.  With  a  GPS  unit  issuing  data  at  a  slower  rate  than  an  IMU  unit  the 
algorithm  must  be  able  to  integrate  the  IMU  data  at  the  required  higher  rate  and  then 
periodically  be  able  to  fuse  the  resulting  data  with  GPS  data.  This  requirement  means 
other  data  sources  also  should  be  easily  accommodated  with  respect  to  their  data  rates. 
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5.  INS  Integrating  Algorithm 

With  the  above  in  mind  the  algorithm  chosen  for  the  integration  of  IMU  data  is  that 
defined  by  Miller  (ref  1).  The  following  sections  describe  in  some  detail  Miller's 
algorithm. 

5.1  Algorithm  Strategy 

Miller's  algorithm  breaks  the  various  processes  up  according  to  relative  required  rates 
of  calculation.  This  provides  a  natural  means  of  incorporating  GPS  data  at  an 
appropriate  rate. 

Three  rates  are  used: 

Fast  Rate: At  the  fastest  rate  raw  IMU  data  is  adjusted  to  account  for  inherent 
biases  then  integrated  to  give  a  representation  of  attitude  and  velocity  and  in  the 
inertial  frame.  The  velocity  represents  only  that  accumulated  from  forces  since  the 
last  iteration  of  calculations  at  the  intermediate  rate. 

Intermediate  Rate:At  the  intermediate  rate  inertial  frame  attitude  is  adjusted  to 
allow  for  Earth  rotation.  The  velocity  increment  since  the  last  intermediate 
calculations  due  to  inertial  forces  is  then  converted  to  the  navigation  frame.  The 
total  velocity  increment  in  the  navigation  frame  needs  to  take  into  accoxmt  velocity 
changes  due  to  gravity  and  corriolis  effects.  The  velocity  increment  due  to  these 
effects  is  calculated  at  this  rate  and  added  to  the  inertial  velocity  increment  to  give 
the  total  velocity  increment  since  the  last  intermediate  calculations.  Total  velocity 
is  then  updated  and  intergrated  to  give  the  change  in  postion  in  the  navigation 
frame  over  the  last  intermediate  period. 

Slow  RaterAt  the  slow  rate  latitude,  longitude,  gravity,  and  Earth  radius  are 
updated. 

The  form  of  data  available  at  the  intermediate  rate  from  the  INS  algoritm  is  compatible 
with  that  from  the  GPS.  This  means  that  it  would  be  appropriate  to  perform  any  INS 
and  GPS  data  fusion  algorithm  at  a  rate  no  greater  than  the  intermediate  rate. 


5.2  Attitude  Representation 

MiUer  utilises  quaternions  as  the  method  for  representing  attitude  of  the  IMU.  The 
following  gives  a  basic  description  of  the  particular  form  of  quaternions  used  by 
Miller.  Appendix  1  provides  some  further  detail  on  their  definition  and  properties. 

A  quaternion  uses  four  parameters  to  describe  the  rotation  from  one  set  of  axes  to 
another.  In  essence  it  consists  of  a  three  element  vector  that  is  common  to  both  axes 
and  a  scalar  value  that  defines  the  rotation  about  that  vector. 

The  governing  differential  equation  for  rotation  in  terms  of  quaternions  defined  in 
Appendix  1  is 


3 


DSTO-GD-0081 


where  Q  is  the  quaternion  and  ©  is  angular  velocity  (which  can  be  represented  as  a 
quaternion  with  a  zero  scalar  term),  and  *  is  the  quaternion  multiplication  operator 
(see  Appendix.  1). 

For  a  system  where  discrete  sampling  is  carried  out  a  means  of  calculating  Q{t  +  d)t)  is 
required.  Miller  describes  several  methods  to  obtain  a  solution  for  this  including  3rd 
order  Taylor  Series  and  4th  order  Rimge-Kutta  (see  Appendix  2).  Miller  also  describes 
the  following  solution. 

Qit+dt)  =  Q(t)*e 

where  0  is  a  rotation  quaternion  defined  as 

Q  =  C,QS  with  C  =  cos(|0o),  5"  =  (l/0o)sin(^0o),  and  0/  =0-0. 

The  "rotation  vector"  0  represents  the  incremental  rotation  of  IMU  relative  to  the 
inertial  frame  during  the  update  period  8r.  To  obtain  0  Miller  assumes  that  during  the 
update  period  the  outputs  from  the  gyros  can  be  described  by  a  second  order 
pol5momial  which  requires  two  samples  during  the  period.  The  following  is  the 
numerical  solution  for  0  in  which  the  very  small  triple  vector  product  is  neglected. 

0  =  5, +52+1(5, xSj) 

where  5,  and  5  2  are  the  incremental  gyro  samples  taken  at  the  mid-point  and  end¬ 
point  of  the  update  period  respectively. 

In  order  to  keep  processing  time  down  when  calculating  the  rotation  quaternion  0 
approximations  are  made  for  C  and  S.  Miller  proposes  the  use  of  either  a  3rd  order 
series  expansion  for  sine  and  cosine,  or  a  modified  2nd  order  expansion,  claiming 
comparable  performance.  The  3rd  order  expansion  has  C  =  l-|0-0,  and 
5  =  ^-:^0-0,  while  for  the  modified  2nd  order  expansion  C  =  1- jy0  -0  and  5=  0.5. 

When  iterated  at  the  fast  rate  the  above  method  for  calculating  Q(t  +  dt)  provides  a 
representation  for  the  attitude  of  the  IMU  over  time  in  the  inertial  frame.  Conversion 
of  Q  to  be  relative  to  an  Earth  based  frame  is  carried  out  at  the  intermediate  rate.  The 
numerical  equation  to  do  this  is 

where  j^t,)S  is  the  conjugate  of  the  rotation  quaternion  9  for  the  rotation 

of  the  Earth  base  frame  with  respect  to  inertial  space  over  the  period  of  the 
intermediate  rate  tj,  and  equals  the  "rotation  vector"  for  the  same.  Note  that  the 
Earth  base  frame  is  usually  defined  on  the  surface  of  the  Earth.  Therefore  ©  wUl  be  a 
fxmction  of  latitude  and  thus  needs  to  be  updated  periodically  as  the  IMU  moves 
across  the  Earth's  surface.  Miller  does  this  at  the  intermediate  rate. 

Periodically  it  is  necessary  to  correct  scale  errors  (see  Appendix  1)  that  accumulate  in 
the  quaternion.  This  is  done  at  the  intermediate  rate  by  normalising  the  quaternion.  In 
order  once  again  to  keep  the  computational  time  down  Miller  uses  the  following  to 
normalise  Q . 
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ez =1.5- 0.52  ef 


1=0 

Q^QL.Q 


5.3  Linear  Motion 

Like  attitude,  linear  motion  can  be  split  into  motion  caused  by  body  external  forces 
and  those  caused  by  gravitational  effects.  Earth  rotation  and  curvature. 

The  velocity  of  the  body,  rotating  with  respect  to  an  inertial  frame,  caused  by  external 
forces,  is  given  by  the  equation 

V  =F-Q  V 

^ IF  '  '•if 

where  is  the  skew  symmetric  matrix  for  body  rotation  relative  to  the  inertial  frame 
and  F  is  the  sum  of  the  external  forces.  In  obtaining  a  solution  to  this  equation  for  the 
fast  rate  calculations.  Miller  claims  that  it  is  a  satisfactory  compromise  between 
computational  load  and  accuracy  to  assume  angular  velocity  and  external  forces  are 
constant  over  the  calculation  period.  The  resulting  numerical  solution  is 

V,^^V,^+A-ex(V,^+iA) 

where  A  =  ^¥dt  is  the  accelerometer  output,  and  0  is  the  "rotation  vector".  With  Vjp 

set  to  zero  at  the  start  of  an  intermediate  period,  iteration  at  the  fast  rate  gives  the 
incremental  change  in  velocity  over  the  intermediate  period. 

For  the  velocity  of  body  relative  to  the  Earth,  caused  by  gravitation  and  axes  rotation 
over  the  intermediate  period,  the  equation  is 

'^EG  ~ 

where  g  is  the  gravity  vector  and  and  are  the  skew  symmetric  matrices  for, 
respectively,  the  rotation  of  the  Earth  relative  to  the  inertial  frame,  and  the  rotation  of 
the  navigation  frame  relative  to  the  Earth  frame.  In  this  implementation  the  Earth  and 
navigation  frames  are  the  same.  The  numerical  solution  to  this  is 

'^EG  ~  &i  ~  0-^IE  "*■ 

where  <-  +  V^g)  is  the  total  velocity  relative  to  the  Earth  calculated  using 

NpQ  from  the  previous  intermediate  calculations. 

Position  is  updated  using  X  <-  X  +  {V^  +  ^  )}f . . 
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6.  Software  Details 

The  author  has  made  several  implementations  of  the  algorithms  discussed  in  chapter 
5.  These  can  be  divided  into  two  main  groups,  those  that  accept  real  time  IMU  data 
and  those  that  take  IMU  data  that  has  been  previously  stored.  They  have  all  been 
written  in  C++. 

The  following  sections  detail  the  various  implementations.  Text  written  in  itahcs  is 
explained  further  in  Appendix  3. 

6.1  IMU  Sampling 

The  IMU  (in  this  case  a  GlClOO)  provides  accelerometer  and  gyro  output  at  a  rate  of 
600  Hz.  The  fast  rate  calculation  period  requires  gyro  samples  at  the  mid  point  and  end 
of  the  period  and  accelerometer  samples  at  the  end  of  the  period.  This  means  that  the 
highest  possible  rate  of  calculations  with  the  GlClOO  is  300  Hz.  In  general  2n 
(n  =  1, 2, 3...)  IMU  Blocks  are  needed  for  the  Fast  Data  Block.  For  example,  on  a  33  MHz 
386  PC  it  was  found  that  a  minimum  of  4  IMU  Blocks  were  needed  for  fast  rate 
calculations  in  order  to  avoid  computational  overload,  giving  a  maximum  update  rate 
of  150  Hz.  With  the  greater  coiriputational  over-heads  of  incorporating  GPS  data  more 
IMU  Blocks  per  fast  rate  calculations  may  be  needed  or  a  faster  processor  used. 


6.2  Program  Descriptions 

I_G_#???.CPP  is  the  filename  format  for  the  main  programs.  #  is  either  R  for  the 
program  using  real  time  input  data,  or  F  for  input  data  read  from  a  file.  ???  refers  to  the 
particular  version. 

The  following  fists  the  source  file  names  for  the  various  implementations  together  with 
a  brief  description  of  the  file's  purpose.  Support  source  files  are  also  fisted. 

I_G_F1.CPP 

This  version  only  processes  IMU  data  stored  in  a  file.  It  features  the  following: 

•  Code  that  handles  IMU  data  blocks  that  may  or  may  not  include  a  time  stamp. 

•  Determination  of  the  IMU  biases,  ie  on  the  accelerometers  and  gyros  before 
processing  begins  and  again  at  regular  periods  during  processing.  Biases  are 
calculated  by  simple  averaging  of  IMU  data  over  a  period  of  time.  This  is 
implemented  by  the  user  supplying  two  parameters  at  rtm  time.  One  for  the 
period  over  which  averaging  takes  place,  and  the  other  for  the  period  at  the  end  of 
which  biases  are  updated.  The  following  shows  how  this  occurs  during 
processing. 
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u 


c  I  c 

start 

processing 


u 


\ 


c 


where  u  =  the  update  period  and  c  =  the  bias  calculation  period,  both  being 
user  defined  at  run-time. 

Note  that  with  averaging  it  is  essential  the  IMU  is  stationary  during  the  bias 
calculation  period.  For  this  reason  in  dynamic  tests  recalculation  is  not 
practical.  In  these  circumstances  u  is  set  to  a  value  greater  than  the  duration  of 
the  test. 

In  section  3  it  was  stated  that  the  fast  rate  calculations  require  gyro  samples  to  be  taken 
at  the  middle  and  end  of  a  Fast  Data  Block,  and  accelerometer  samples  at  the  end.  A 
variation  of  this  is  implemented  where  for  the  gyros  samples  within  the  each  half  of 
the  Fast  Data  Block  are  averaged  and  the  accelerometer  samples  averaged  over  the 
entire  block  before  being  used  in  calculations.  Assuming  actual  changes  in  rates  and 
accelerations  during  a  Fast  Data  Block  are  less  than  IMU  noise  then  the  signal  to  noise 
ratio  wih  be  improved.  The  impact  of  this  becomes  greater  as  the  number  of  IMU 
blocks  per  Fast  Data  Block  increases. 

I_G_F2_A.CPP 

This  version  processes  both  GPS  and  IMU  data.  All  the  features  of  version  I_G_F1.CPP 
are  contained  in  this  version  along  with  the  following: 

•  The  GPS  data  used  is  taken  from  a  file  containing  output  data  from  an  Auspace 
Multinav  receiver. 

•  The  method  of  incorporating  the  GPS  data  is  to  simply  update  periodically  the 
estimated  position,  velocity  and  attitude  information  being  generated  by  the 
integration  of  IMU  data.  This  is  achieved  by  averaging  GPS  data  for  a  period  prior 
to  the  moment  when  updating  occurs.  INS  position  and  velocity  estimates  are 
reset  to  those  of  the  GPS  average  and  the  quaternion  to  represent  a  level 
orientation  while  maintaining  INS  estimated  heading.  It  should  be  noted  that  by 
using  an  averaging  technique  for  the  GPS  data  that  operation  is  limited  to  the 
static  applications  unless  the  averaging  period  is  set  to  one  second,  in  which  case 
only  one  GPS  sample  is  taken,  thus  allowing  dynamic  appHcations. 

•  It  is  possible  to  set  an  averaging  period  of  the  GPS  data  that  is  greater  than  the 
update  period. 

I_G_F2_T.CPP 

This  version  is  the  same  as  I_G_F2_A.CPP  with  the  only  exception  being  the  GPS  data 
was  taken  from  a  Trimble  receiver  hence  the  GPS  input  data  is  in  a  different  format. 
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I_G_F3.CPP. 

This  version  is  designed  to  test  the  algorithm's  performance  without  the  effects  of 
inaccuracies  in  the  input  data  from  the  IMU.  Consequently  simulated  IMU  data  for 
known  motion,  eg  coning,  can  be  generated  and  stored  in  a  file  for  use  in  this  version. 
The  following  describe  its  differences  from  the  version  above. 

•  The  IMU  input  data  file  has  a  different  format  of  IMU  data,  using  floating  point 
numbers. 

•  No  bias  calculations  are  performed. 

•  No  GPS  data  is  used. 

I_G_R1.CPP 

This  version  processes  real  time  data  supplied  from  the  GlClOO  IMU  only.  It  is  a  basic 
version  with  few  added  features.  The  following  details  its  main  features: 

•  IMU  data  is  captured  through  the  PCs  DMA  operation  via  a  purpose  buHt 
interface. 

•  Accelerometer  and  gyro  biases  are  calculated  by  averaging  data  over  a  set  period 
while  the  IMU  is  stationary  and  comparing  them  against  accelerometer  and  gyro 
values  calculated  for  the  IMU's  location  and  orientation.  During  this  process  the 
DMA  handles  one  IMU  Block  of  data  at  a  time. 

•  For  the  integration  process  the  fast  rate  calculations  are  performed  only  after  there 
has  been  accumulated  in  a  buffer  via  the  DMA  all  the  IMU  data  generated  during 
an  intermediate  rate  calculation  period.  When  the  buffer  is  fuU  the  DMA  initiates  an 
interrupt  which  in  turn  flags  the  commencement  of  fast  rate  calculations. 

I_G_R2.CPP 

This  version  differs  from  I_G_R1.CPP  in  one  important  way  which  is  explained  below 

•  As  in  I_G_R1.CPP  two  buffers  are  used  in  the  integrating  process,  however  their 
size  is  such  that  they  can  only  store  the  data  of  one  Fast  Data  Block  each.  When  an 
interrupt  is  initiated  after  a  buffer  is  full  the  fast  rate  calculations  are  carried  out 
immediately  while  the  other  buffer  is  being  filled.  When  enough  fast  rate 
calculations  have  been  done  an  intermediate  rate  calculation  is  performed. 

A  drawback  with  this  version  is  that  with  the  smaller  buffers  than  in  the  I_G_R1 
version  there  is  less  time  in  which  to  perform  extra  calculations.  Thus  when  GPS  is 
introduced  into  the  algorithm  this  method  is  more  likely  to  require  longer  IMU 
sampling  periods.  On  the  other  hand  the  advantage  of  this  version  is  that  the  delay 
between  the  time  the  intermediate  calculations  are  completed  and  the  time  for  which 
they  are  valid  will  be  less. 

IMU.CPP 

This  file  contains  function  definitions  that  are  specific  for  the  operation  of  the  GlClOO 
IMU.  The  functions  are  only  required  by  the  real  time  main  programs  I_G_R???.CPP. 
The  ftmction  declarations  are  made  in  the  IMU.H  file. 
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DMA.CPP 

This  file  contains  a  set  of  functions  to  facilitate  the  operation  of  the  PC's  DMA.  Some  of 
these  functions  are  required  only  by  the  real  time  main  programs  I_G_R???.CPP.  The 
function  declarations  are  made  in  the  DMA.H  file. 

INTRPT.CPP 

This  file  contains  a  set  of  functions  to  facilitate  the  operation  of  the  PC's  Interrupt 
ports.  Some  of  these  functions  are  required  only  by  the  real  time  main  programs 
I_G_R???.CPP.  The  fimction  declarations  are  made  in  the  INTRPT.H  file. 


6.3  Main  Program  Structure 

The  following  sections  describe  the  structures  of  the  various  main  program  versions 
listed  above. 

6.3.1  Common  Assumptions 

For  all  the  main  programs  versions  Hsted  above  there  are  some  common  assumptions 
made,  which  are: 

•  The  initial  orientation  of  the  IMU  must  be  such  that  the  Z-axis  is  pointing  down. 

•  The  IMU  data  is  assumed  to  have  been  generated  at  600  Hz. 

6.3.2  Versions  I_G_F1.CPP,  I_G_F2_A,  and  I_G_F2_T 

Figure  1  shows  a  block  diagram  that  describes  the  structure  of  the  main  program  of 
versions  1_G_F1.CPP,  I_G_F2_A.CPP  and  1_G_F2_T.CPP.  The  latter  two  differ  from 
I_G_F1.CPP  primarily  by  the  code  represented  inside  the  dotted  box  after  the  function 
Output_Location(),  in  which  GPS  data  is  incorporated.  Also  for  the  latter  two  versions 
additional  code  has  been  made  to  the  InitialiseQ  function  that  opens  the  GPS  input 
data  file  and  reads  from  it  into  an  array  relevant  data  for  the  first  GPS  data  averaging 
period. 
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This  function  performs  the  following: 

Open  all  input  and  output  files; 

Allocate  memory  for  buffers; 

Read  over  header  info,  in  IMU  input  data  file; 
Obtain  fi’om  user  Bias  Update  and  Calulation 
Periods,  and  IMU  location  and  orientation; 
Calculate  initial  rotation  vector  and  quaternion. 


,  This  function  does  the  following; 

Read  data  from  the  IMU  input  data  file  to  fill 
one  Fast  Loop  Block; 

If  this  data  is  in  the  bias  recalculation  period 
then  the  data  is  stored.  If  it  is  time  to 
update  the  biases  then  this  stored  data  is 
used  to  recalculate  the  biases; 

Performs  the  integration  algorithm  for  the 
quaternion,  and  incremental  change  in 
velocity  since  the  last  call  to 
Intermediate_LoopO; 

Increment  fast  loop  counter  CF. 


This  function  updates  the  following: 
Latitude  and  longitude; 
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consists  of  a  series  of  data  blocks  each  containing  accelerometer  and  gyro  values  and 
an  optional  extra  time  stamp  (see  Appendix  3  under  IMU  Block).  When  the  time  stamp 
is  included  there  is  also  some  header  information  at  the  start  of  the  file.  The  time 
information  is  never  used,  but  the  different  format  of  files  with  or  without  time  stamps 
has  to  be  dealt  with.  This  is  done  by  defining  TIME_DATA_FLAG  as  1  in  the  include 
file  1_G_FILE.H  when  the  time  stamp  is  used. 

IMU_OUT.DAT 

This  is  a  binary  output  file  that  stores  the  accelerometer  and  gyro  values  (before 
conversion  from  GlClOO  units)  actually  used  in  the  fast  rate  calculations.  Nine  integer 
values  are  stored  for  each  fast  rate  calculation.  In  order  they  are  x,  y,  z  accelerometer 
values,  gyro  values  at  mid-point  of  sample,  and  gyro  values  at  end-point  of  sample. 

INSGPS.DAT 

This  text  output  file  stores  various  values  during  the  execution  of  the 
Output_Location()  function,  that  have  been  generated  by  the  program.  Eleven  floating 
point  numbers  are  written  to  file  and  in  order  are:  Time  (s)  since  the  start  of  data 
processing  (referenced  to  the  input  data,  not  real  time);  Displacement  (m)  from  initial 
position  in  north,  east,  and  altitude  directions;  Current  velocity  (m/s)  relative  to  the 
Earth  in  north,  east,  and  down  directions;  Quaternion  scalar  value  Qg,  and  vector 
values  Qj,  Q2,  and  Q3. 


For  versions  I_G_F2. A.CPP  and  I_G_F2_T.CPP  an  additional  input  file  is  opened  for 
reading  GPS  data  named  GPS_IN.DAT.  The  Trimble  and  Auspace  GPS  receivers  that 
generate  the  data  stored  in  the  file  use  completely  different  formats  and  provide  more 
information  than  is  required  by  these  programs.  In  Appendix  3  the  relevant  data  block 
formats  are  given  under  Auspace  Data  and  Trimble  data.  In  the  Trimble  data  there  is  no 
velocity  value  for  the  vertical  direction.  This  means  that  for  version  I_G_F2_T.CPP  the 
vertical  velocity  estimate  does  not  get  altered  when  GPS  updating  takes  place  and  thus 
remains  solely  the  product  of  IMU  integration. 

The  following  user  input  requirements  are  the  same  for  aU  three  versions. 

•  Bias  update  period. 

•  Bias  calculation  period. 

•  Initial  latitude,  longitude,  and  altitude. 

•  Initial  heading 

For  the  versions  including  GPS  data  the  period  when  GPS  data  is  used  to  update  the 
position,  velocity  and  quaternion  must  also  be  suppHed  by  the  user. 
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6.3.3  Version  I_G_F3.CPP 

Figure  2  shows  the  general  structure  of  version  I_G_F3.CPP.  Comparing  to  figure  1  it  can  be 
seen  that  structurally  it  differs  from  version  I_G_F1.CPP  by  only  the  removal  of  the 
function  that  calculates  the  IMU  biases. 


Additional  differences  not  shown  in  the  figures  are  as  follows. 

•  As  pointed  out  in  section  6.2  this  version  uses  floating  point  niunbers  in  the  IMU 
input  data  file  (which  is  named  TEST_1N.DAT).  The  format  of  this  data  is  shown 
in  Appendix  3  tmder  IMU  Test  Block. 

•  An  additional  input  data  file  named  I_G_INIT.DAT  is  used  to  read  in  some  of  the 
data  that  the  user  would  otherwise  be  required  to  enter  on  execution.  The  file  is  a 
text  file  and  the  data  stored  in  it,  in  order,  is  the  initial  latitude,  longitude,  altitude, 
and  the  heading.  This  data  is  placed  all  on  one  line  and  separated  by  "#"  symbols. 
For  all  bar  altitude  there  are  two  values  separated  by  a  space  for  degrees  and 
minutes  with  the  latter  being  a  decimal  number  if  necessary.  For  example: 

•  -34  42.603#  138  38.435#  25#  0  0# 

•  With  no  biasing  or  GPS  data,  and  I_G_IN1T.DAT  being  used,  this  version  does  not 
require  any  data  to  be  entered  by  the  user  during  execution. 

6.3.4  Version  I_G_R1.CPP  and  I_G_R2.CPP 

Figure  3  shows  the  structure  of  main  section  and  its  interrupt  functions  of  I_G_R1.CPP, 
while  the  same  for  version  1_G_R2.CPP  is  shown  in  figure  4.  Some  functions  relating  to 
the  interrupt  and  DMA  for  the  IMU  have  not  been  described  specifically.  These 
functions  are  defined  in  IMU.CPP,  DMA.CPP,  and  INTRPT.CPP  where  further  details 
of  their  operation  are  given. 

For  both  versions  there  are  many  common  aspects.  These  are  dealt  with  first  and  the 
unique  aspects  are  described  directly  after  the  applicable  figures. 

There  are  two  output  files  generated.  These  are  called  IMU_OUT.DAT  and 
1NSGPS.DAT  and  are  identical  to  the  output  files  of  the  same  name  described  in 
section  6.3.2. 

The  only  user  input  required  during  execution  is  the  initial  location  of  the  IMU,  ie 
latitude  longitude  and  altitude,  and  the  heading. 

The  IMU  interface  is  coimected  to  DMA  channel  7  and  Interrupt  channel  10.  The  basic 
operational  relationship  between  the  IMU  interface,  DMA,  and  Interrupt  is  as  follows. 
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This  function  performs  the  following: 

Open  all  input  and  output  files; 

Read  from  file  intitial  IMU  position  and 
orientation; 

Allocate  memory  for  buffers; 

Read  over  header  info,  in  IMU  input  data  file 
Obtain  from  user  Bias  Update  and  Calulation 
Periods; 

Calculate  initial  rotation  vector  and 
quaternion. 


This  function  does  the  following: 

Update  the  quaternion  to  allow  for  earth 
rotation; 

Normalise  quaternion  to  correct  scale  error; 

Update  the  velocity  relative  to  the  earth 
since  the  start,  and  the  change  in  position 
sinee  the  last  Slow_LoopO  call; 

Increment  intermediate  loop  counter  Cl. 


This  function  outputs  to  the  screen  and 
the  output  data  file  various  parameters. 


Note:  Block  labels  that  end  with  0 
indicate  hmction  names  in 
the  program; 

FL_PER_IL  equals  the  number  of  fast 
loops  requbed  per  intermediate  loop; 

1L_PER_SL  equals  the  number  of 
intermediate  loops  required  per  slow  loop. 


Output_LocatioaO 

_ 

y'  while 

no  key  hit 

n 

[  END  ) 


This  function  does  the  following: 

Read  data  from  the  IMU  input  data  file  to  fill 
one  Fast  Loop  Block; 

If  this  data  is  in  the  bias  recalculation  period 
then  the  data  is  stored.  If  it  is  time  to 
update  the  biases  then  this  stored  data  is 
used  to  recalculate  the  biases; 

Performs  the  integration  algorithm  for  the 
quaternion,  and  incremental  change  in 
velocity  since  the  last  call  to 
Intermediate_LoopO; 

Increment  fast  loop  counter  CF. 


This  function  updates  the  following: 
Latitude  and  longitude; 

Altitude; 

Earth  constants; 

Earth  rotation  vector. 


This  function  cleans  up  before  the 
the  program  is  terminated. 


Figure  2.  Flow  diagram  for  I_G_F3.CPP 
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This  function  performs  the  following: 

Setup  interrupt  vector  to  Bias_IntrptO; 
Holds  in  a  loop  until  BiasJntrptQ  has 
been  called  the  required  number  of  times; 
Prevent  further  IMU  data  transfers; 
Calculate  biases. 


[Bias_IiitrptO] 


Store  IMU  Block  data 
from  DMA  buffer 


X 


Advance  counter 


X 


Clear  the  interrupt 


[end  Interrupt] 


Holding  loop" 

This  function  does  the  following: 

Update  the  quaternion  to  allow  for  earth 
rotation; 

Normalise  quaternion  to  correct  scale  error: 
Update  the  veloeity  relative  to  the  earth 
sinee  the  start,  and  the  change  in  position 
since  the  last  SIow_LoopO  call; 
Increment  intermediate  loop  counter  Cl. 


This  function  outputs  to  the  screen  and 
the  output  data  file  various  parameters. 


Note:  Block  labels  that  end  with  {) 
indicate  function  names  in 
the  program; 

FL_PER_IL  equals  the  number  of  fast 
loops  required  per  intermediate  loop; 

IL_PER_SL  equals  the  number  of 
intermediate  loops  required  per  slow  loop; 

bufrer_filled  equals  1  when  an  buffer 
has  been  filled. 


c 


END 


D 


This  function  performs  the  following: 

Open  all  input  and  output  files; 

Setup  interrupt  mode; 

Save  old  interrupt  vectors; 

Allocate  memory  for  buffers; 

Setup  DMA  transfer  mode; 

Obtain  from  user  IMU  location  and  orientation; 
Calculate  initial  rotation  vector  and  quaternion. 

This  function  reads  from  the  IMU  Block  buffer 
the  accelerometer  and  gyro  values  and  diplays 
them  on  the  screen. 


[iMUJntrptQ] 


Setup  DMA  to  write  to 
Intrm.  Data  Block  buffer 

TX 

L. 

_ 

[f/  while 

notbuffer_filIcd 

n 

Reset  DMA  to  write 
to  other 

Intrm.  Data  Block  buffer 
I  . 


buffer  filled  =  1 


X 


Clear  the  interrupt 


[  END  Interrupt 

This  function  does  the  following  for 
F_L_PER_IL  times: 

Read  IMU  data  from  Intrm.  Data  Block  buffer 
for  one  Fast  Data  Block; 

Perform  the  integration  algorithm  for  the 
quaternion,  and  incremental  change  in 
velocity  since  the  last  call  to 
Intermediate_LoopO; 


This  function  updates  the  following: 
Latitude  and  longitude; 

Altitude; 

Earth  constants; 

Earth  rotation  vector. 


DelnitialiseQ 

Before  program  is  terminated  this  function; 
Restores  old  interrupt  vectors; 

Restorse  old  interrupt  mode; 

Closes  open  files. 


Figure  3.  Flow  diagram  for  version  1_G_R1.CPP. 
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This  function  performs  the  following: 

Setup  interrupt  vector  to  Bias_IntrptO; 
Holds  in  a  loop  until  Bias_IntrptO  has 
been  called  the  required  number  of  times: 
Prevent  further  IMU  data  transfers; 
Calculate  biases. 


[Bias_IntrptO] 


Store  IMU  Block  data 
from  DMA  buffer 


Advance  counter 


X 


Clear  the  interrupt 


[end  Interrupt] 


Holding  loop' 


This  function  does  the  following: 

Update  the  quaternion  to  allow  for  earth 
rotation; 

Normalise  quaternion  to  correct  scale  error; 
Update  the  velocity  relative  to  the  earth 
since  the  start,  and  the  change  in  position 
since  the  last  Slow_LoopO  call; 
Increment  intermediate  loop  counter  Cl. 


This  function  outputs  to  the  screen  and 
the  output  data  file  various  parameters. 


Note:  Block  labels  that  end  with  Q 
indicate  function  names  in 
the  program; 

FL_PER_IL  equals  the  number  of  fast 
loops  required  per  intermediate  loop; 

IL_PER_SL  equals  the  number  of 
intermediate  loops  required  pa*  slow  loop; 


This  function  performs  the  following: 

Open  all  input  and  output  files; 

Setup  interrupt  mode; 

Save  old  interrupt  vectors; 

Allocate  memory  for  buffers; 

Setup  DMA  transfer  mode; 

Obtain  from  user  IMU  location  and  orientation; 
Calculate  initial  rotation  vector  and  quaternion. 

This  function  reads  from  the  IMU  Block  buffer 
the  accelerometer  and  gyro  values  and  diplays 
them  on  the  screen. 


[iMUJntrptO] 

^  ' '  I  . . 

Reset  DMA  to  write 
to  other 

iFast  Data  Block  buffer 


copy  buffer 


FAST  LOOP 
CALCULATIONS 


CF-H- 


Clear  the  interrupt 


X 


END 


Interrupt] 


Operations  carried  out  here  are: 

Read  IMU  data  from  Fast  Data  Block  buffer; 
Perform  the  integration  algorithm  for  the 
quaternion,  and  incremental  change  in 
velocity  since  the  last  call  to 
Intermediate_LoopO; 


This  function  updates  the  following: 
Latitude  and  longitude; 

Altitude; 

Earth  constants; 

Earth  rotation  vector. 


Before  program  is  terminated  this  function: 
Restores  old  interrupt  vectors; 

Restorse  old  interrupt  mode; 

Closes  open  files. 


Figure  4.  Flow  diagram  for  version  I_G_R2.CPP. 


DMA  channel  7  is  set  up  to  write  transfer,  autoinitialise,  and  single  transfer  modes 
while  the  Interrupt  controller  is  set  to  Special  FuUy  Nested  Mode  (for  more  details 
refer  to  Section  3  and  5  of  reference  2).  This  is  achieved  through  the  function  calls 
Set_DMA_Mode()  and  Initialise_IC()  respectively.  Before  transfers  take  place  DMA 
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channel  7  is  given  an  address  of  a  bxiffer  to  transfer  to  and  the  number  of  words  to 
transfer  through  the  function  call  Load_IMU_DMA(),  and  Interrupt  channel  10  has  its 
vector  set  to  an  interrupt  function  through  a  caU  to  Set_Interrupt_Vector().  Wilh  this 
done  transfers  can  now  take  place.  When  the  interface  has  IMU  data  ready  to  send  it 
notifies  the  DMA  to  commence  transfer.  Once  the  DMA  has  cotmted  the  required 
number  of  transfers  it  reinitialises  itself  and  notifies  the  IMU  interface.  The  Interface 
responds  by  requesting  an  interrupt  so  that  the  interrupt  frmction  can  then  operate  on 
the  data  stored  in  the  buffer.  The  process  then  is  repeated. 

There  are  two  different  sized  buffers,  one  is  for  the  calculation  of  biases  while  the  other 
is  for  the  actual  integration  process.  Likewise  there  are  two  separate  interrupt 
fimctions. 

For  the  bias  calculations  the  buffers  size  is  such  that  it  wiU  take  one  IMU  Block  of  data. 
The  name  of  the  interrupt  hmction  is  Bias_Intrpt().  The  duration  in  seconds  over  which 
biases  are  calculated,  and  hence  the  number  of  times  Bias_Intrpt()  is  called,  is 
determined  by  BIAS_TIME  which  is  defined  in  I_G_REAL.H. 

For  the  integration  process  in  version  I_G_F1.CPP  two  separate  buffers  are  allocated, 
each  capable  of  storing  an  Intermediate  Data  Block.  The  program  sets  up  the  DMA  to 
alternate  between  the  buffers  as  each  is  filled.  When  a  buffer  is  full  IMU_Intrpt()  is 
requested  to  swap  DMA  buffers.  On  return  from  IMU_Intrpt()  the  program  drops  out 
of  a  holding  loop  to  commence  integration  calculations.  First  the  fast  rate  calculations 
are  repeated  until  all  the  data  in  the  most  recently  filled  buffer  has  been  used.  Next  one 
set  of  intermediate  rate  calculations  are  performed  and  finally  if  sufficient  intermediate 
rate  calculations  have  already  occurred,  a  slow  rate  calculation  is  performed.  When 
these  have  aU  been  completed  the  program  returns  to  the  holding  loop  to  await  the 
next  IMU_Intrpt()  request.  While  processing  is  taking  place  on  the  data  in  one  buffer, 
the  other  is  being  filled.  Data  will  be  lost  if  IMU_Intrpt()  is  requested  before  the 
completion  of  all  the  fast  rate  calculations  on  the  data  in  the  previously  filled  buffer. 

For  the  integration  process  in  version  I_G_R2.CPP  two  separate  buffers  are  allocated, 
each  capable  of  storing  a  Fast  Data  Block.  The  program  sets  up  the  DMA  to  alternate 
between  the  buffer  as  each  is  filled.  When  a  buffer  is  full  IMU_Intrpt()  is  requested. 
This  fimction  swaps  the  DMA  buffers  and  then  performs  one  set  of  fast  rate 
calculations.  When  IMU_Intrpt()  has  finished  the  program  returns  to  a  holding  loop. 
After  the  required  number  of  iterations  of  IMU_Intrpt()  have  occurred  the  program 
falls  out  of  the  holding  loop  to  do  the  intermediate  rate  calculations  and  if  sufficient 
intermediate  rate  calculations  have  already  occurred,  the  slow  rate  calculations  are 
performed.  The  program  must  complete  all  this  and  return  to  the  holding  loop  before 
the  next  IMU_Intrpt()  is  requested  otherwise  data  will  be  lost. 

6.4  Unresolved  Problems 

In  the  real  time  versions  a  problem  was  encountered  in  which  occasionally  the 
program  would  lock  up  the  computer.  It  appeared  that  critical  memory  was  being 
overwritten,  probably  by  the  DMA.  Investigation  found  that  there  are  delays  of 
random  frequency  and  duration  during  processing  and  when  the  delays  are  long 
enough  the  calculations  are  not  complete  on  a  particular  buffer's  data  when  an 
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interrupt  request  to  change  buffers  occurs.  The  problem  can  be  avoided  by  increasing 
the  number  of  IMU  Blocks  per  set  of  fast  rate  calculations  and/or  the  number  of  fast 
rate  calculation  sets  per  intermediate  rate  calculation  set.  In  effect  this  provides  more 
processing  time  for  the  calculations  thus  accommodating  tiie  delays  as  part  of  the 
processing  time.  This  is  not  an  entirely  desirable  solution  since  the  slower  processing 
rate  is  counter  productive.  Investigations  continue. 


6.5  Planned  Modifications 

The  following  functional  additions  and  or  changes  are  planned,  which  will  result  in 
new  main  program  versions  that  either  source  their  data  in  real  time  or  from  file. 

•  A  new  calibration  routine  for  initialising  the  IMU  that  wiU  use  Kalman  Filtering 
techniques. 

•  Inclusion  of  integrity  checks  that  wiU  monitor  the  following  for  tmexpected 
results: 

1.  The  sensor  data; 

2.  The  global  solution ; 

3.  Algorithms  for  the  fusion  of  data,  ie  Kalman  Filters. 

Bad  results  in  any  of  the  above  will  lead  to  the  rejection  of  the  contributing  data. 
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Appendix  1 

Quaternion  Definition  and  Properties 


A  quaternion  uses  four  parameters  to  define  the  rotation  of  one  3  dimensional 
coordinate  system  relative  to  another.  Miller  defines  it  as: 

Q  =  C,QS 

where:  C  =  cos(y0o);  5  =  (l/0o)sin(y0o);  0  =  (0,,02,03)  is  the  "rotation  vector" 
(assuming  0j,02,03  are  small);  and  0o  =  (0j^  +02^  +03^)^. 

The  physical  interpretation  of  this  is  that  of  a  rotation  through  an  angle  0o  measured 
from  reference  to  body  axes,  about  a  unit  vector  defined  (in  both  axes  systems)  by 
0/00. 

Normalisation  of  Quaternions 

With  the  quaternion  defined  as  above  the  sum  of  the  squares  of  the  four  parameters  is 
unity.  The  square  root  of  this  is  often  called  the  "length"  of  the  quaternion. 

Should  the  length  change  from  unity  then  a  "scale  error*'  has  taken  place.  This  can  be 
corrected  by  "normalisation",  ie  by  dividing  the  quaternion  by  its  length. 

Quaternion  "Multiplication" 

Given  the  quaternions  A  =  Ag,  A  and  B  =  Bq,B,  then  their  product  C  =  Cq , C  is 
C  =  A*B  =  (AgBg  -A-B),{AgB  +  ABg  +(A X  B)} . 

To  physically  interpret  this  consider  a  body  where  A  represents  the  rotation  from 
reference  to  body  axes.  If  the  body  is  rotated  relative  to  the  reference  frame  so  that  B 
represents  the  rotation  from  the  old  to  new  body  axes,  then  the  quaternion  C 
represents  the  rotation  from  the  reference  to  the  new  body  axes. 

Refer  to  Appendix  2  of  MiQer  for  further  details. 
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Appendix  2 

3rd  Order  Taylor  Series  and  4th  Order  Runge-Kutta 

Solutions 


The  following  are  alternative  solutions  to  jg  (0  =  2  0  (0*  ®  (0  to  0  (f  +  ^)  • 

3rd  Order  Taylor  Series: 

Q{t+h)  =  Q{t)*Uit) 

where:  U  =  C/g ,  U  is  a  quaternion  defined  by; 
t/o=l-KS-5); 

U  =  i6  +  i(6,+62)-i(6-8)6; 

h  =  the  sample  period; 

5  j  is  the  integrated  gyro  sample  from  t  to  (i  +  y/z); 

62  is  the  integrated  gyro  sample  from  (t  +  \h)  to  {t+h); 

6=5,  +62; 

4th  Order  Runge-Kutta: 

With  5,  and  62  defined  as  above  let 

HOq  =  (35,  -82)  and  H©,  =  (5,  +82)  and  £[©2  =  (382  -5,) 

then 

K  =  2Q(t)*H(Oo 
Q^it  +  \h)  =  Q{t)  +  \k^ 

Qiif  +  T^)  =  QiO  +  j^i 

kj  =  Y  02  (^  + 1  ^)*  1 

QYf  +  h)  =  2(0 +  ^2 


and  finally 


^3  =  1 03  (^  +  ^)*  2 

Q(t  +  h)  =  Q{t)  +  \(ko+2(ki  +k^)  +  k^) 
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fast  rate  calculations: 
intermediate  rate  calculations: 

slow  rate  calculations: 
FL_PERJL: 

IL_PER_SL: 

CP: 

Cl: 

IMU  Block: 


DSTO-GD-0081 


Appendix  3 
Terminology 


Where  integration  of  IMU  data  is  performed  in  body 
axes.  Performed  at  highest  frequency  (100-300  Hz). 

Corrects  quaternion  for  Earth  rotation,  converts  to 
Navigation  coordinates  and  integrates  to  location. 
Frequency  is  1-10  Hz. 

Updates  Earth  constants.  Frequency  s  0.1-1  Hz. 

integer.  Number  of  times  fast  rate  calculations  are 
performed  before  intermediate  rate  calculations  are 
initiated. 

integer.  Number  of  times  intermediate  rate  calculations 
are  performed  before  slow  rate  calculations  are  initiated. 

A  counter  used  to  count  the  number  of  times  fast  rate 
calculations  have  been  performed  since  intermediate 
rate  calculations  were  performed  last. 

A  coxmter  used  to  count  the  number  of  times 
intermediate  rate  calculations  have  been  performed 
since  slow  rate  calculations  were  performed  last. 

A  single  block  of  data  from  the  IMU,  24  bytes  long 
(optional  32).  Contains  gyro  and  accelerometer  info  for  3 
axes  plus  an  optional  time  stamp.  The  IMU  Block  detail 
for  the  GlClOO  is  as  follows: 


word  1 

X  accel 

word  2 

X  accel  tag  =  xOOOd 

3 

Z  accel 

4 

Z  accel  tag  =  x0009 

5 

Y  accel 

6 

Y  accel  tag  =  xOOOb 

7 

Y  gyro 

8 

Y  gyro  tag  =  xOOOe 

9 

Zgyro 

10 

Z  gyro  tag  =  xOOOa 

11 

Xgyro 

12 

X  gyro  tag  =  xOOOc 

13 

option  time 

14 

optional  time 

One  word  consists  of  a  2  byte  integer. 


To  convert  accelerometer  values  to  m/s/s  they  must  be 
multiplied  by  1.1963e-l,  and  the  gyros  must  be 
multiphed  by  1.28e-3  to  convert  them  to  rad/ s. 
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Fast  Data  Block: 


Intermediate  Data  Block: 


IMU  Test  Block: 


A  block  of  data  containing  2  n  (n  -  1,  2,  3,...)  IMU 
Blocks  which  is  the  minimum  required  to  perform  a 
single  fast  rate  calculation. 

A  block  of  data  containing  FL_PERJL  number  of  Fast 
Data  Blocks.  This  is  the  amount  of  data  that  must  be 
processed  before  an  Intermediate  rate  calculations  will 
be  performed. 

A  single  block  of  data  containing  3  accelerometer 
values  and  3  gyro  values.  The  values  have  been 
generated  by  a  program  that  calculates  the  exact 
accelerometer  and  gyro  values  for  a  defined  motion. 
The  format  of  the  IMU  Test  Block  is  as  follows: 


word  1 

X  accel 

2 

Y  accel 

3 

Z  accel 

4 

X  Gyro 

5 

Y  Gyro 

6 

Z  Gyro 

Auspace  Data: 


One  word  consists  of  an  8  byte  double  floating  point. 

The  values  are  already  in  m/s/s  and  rad/s  for 
accelerometers  and  gyros  respectively. 

The  relevant  data  required  by  version  I_G_F2_A.CPP  is 
contained  in  "message  100".  This  message  is  in  a  binary 
form.  Its  format  is  as  follows. 


Name 

Type  _ _ 

Data 

1 

Start  Byte 

1  Byte 

02  hex 

2 

Header 

1  Byte 

CC  hex 

3-4 

Receiver  ID 

2  Byte  Integer 

000  -  999  decimal 

5-6 

Time  Stamp,  Week  No. 

2  Byte  Integer 

Standard  GPS  Week  Number 

7-10 

Time  Stamp,  TOW 

4  Byte  Float 

Seconds 

11-15 

Satellite  Numbers 

5x1  Byte  Integers 

Satellites  used  by  KF 

16-19 

Position,  Latitude 

4  Byte  Float 

Radians,  WGS  84 

20-23 

Position,  Longitude 

4  Byte  Float 

Radiarrs,  WGS  84 

24-27 

Position,  Altitude 

4  Byte  Float 

Radians,  WGS  84 

28-31 

Velocity,  North 

4  Byte  Float 

Meters/ Sec 

32-35 

Velocity,  East 

4  Byte  Float 

Meters/ Sec 

36-39 

Velocity,  Up 

4  Byte  Float 

Meters/Sec 

40 

Checksum 

1  Byte  Integer 

Sum  of  bytes  3  to  39 

41 

End  Byte 

1  Byte 

03  hex 
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Trimble  Data:  The  relevant  data  required  by  version  I_G_F2_T.CPP  is 

contained  in  two  lines  of  text,  with  a  constant  number 
of  characters,  generated  by  the  Trimble  receiver.  These 
are  as  follows: 


Latitude 


N/S 

North  or 
.  South  . 


Longitude 


EAV 
East  or 
West 


M/F 

meters 

or  feet 


l#IG|X|G|X|PMh|h|s|s|m|m|,|  1111-111  |,| 

- - 

deg. 


minutes 
(integer)  (decimal) 


i.M  I  M  i-i  M  \M\:rn . itt 


lii 


deg. 

(integer) 


minutes 

(decimal) 


Altitude 

(integer) 


K/M/k  T/M 

knots,  miles/hour,  True  or 
or  kilometers/hour  Magnetic 


|#|G|X|S|0|G|,|h|h|s|s|m|m|,| 


- 

Line  Identifier 


Time 


Speed  over  Ground 
(decimal) 


Heading 
deg.  (integer) 


buffer _filled: 


TIME  DATA_FLAG: 


A  flag  used  in  version  I_G_R1.CPP  that  is  set  to  1  by  an 
interrupt  function  when  an  Intermediate  Data  Block 
has  been  passed  to  memory  via  the  DMA  controller. 

Flag  in  the  file  data  version  of  IMU.FI  (see  sect.  4.1)  that 
when  equal  to  1  allows  the  handling  of  IMU  blocks  that 
include  a  time  stamp  at  the  end. 
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